% st = load(['..' filesep 'traj_ss_lsys.mat']);
st = load('C:\Users\rsinnet\Documents\MATLAB\meen-652-biped\code\traj_ss_lsys.mat');

% figpath = [
%     '..' filesep ...
%     '..' filesep ...
%     'docs' filesep ...
%     'report' filesep ...
%     'figures' filesep];

figpath = 'C:\Users\rsinnet\Documents\MATLAB\meen-652-biped\docs\report\figures\';

A = st.A;
B = st.B;
K = st.K;
% phip = st.phip - min(st.phip);
phip(1,:) = st.phip(1,1,:) - min(st.phip(1,1,:));

[M, dum, N] = size(A);
% zeros_K = zeros(M, N);
% poles_K = zeros(M, N);

Npoints = 35;
W = logspace(-3, 2, Npoints);
[X, Y] = meshgrid(W, phip);

Z = nan(N, Npoints, 2);

for k=1:N
    G = ss(A(:,:,k),B(:,:,k),K(:,:,k),0);
    Gcl = feedback(G,eye(5));
    zeros_K(:, k) = zero(Gcl);
    poles_K(:, k) = pole(Gcl);
    SV = sigma(Gcl, W);
    Z(k, :, 1) = mag2db(min(SV));
    Z(k, :, 2) = mag2db(max(SV));
    
    if k == 1
        sensitiv = sigma(Gcl/G,W);
    end
end

[P dum] = size(poles_K);
[Ze dum] = size(zeros_K);

figure(193);
set(gcf, ...
    'NumberTitle', 'off', ...
    'Name', 'MEEN 652: Closed Loop Poles and Zeros', ...
    'Position', [560 528 560 220], ...
    'PaperPosition', [2 3 4 1.5]);
gh(1:P) = plot(phip, real(poles_K), 'b', ...
    'LineWidth', 1);
[N dum] = size(gh);
hold on
gh(P+1:P+Ze) = plot(phip, real(zeros_K), 'r', ...
    'LineWidth', 1);
hold off
xlabel('$p_{hip}^{x}$ (meters)', ...
    'interpreter', 'latex', ...
    'FontSize', 12);
ylabel('$Re\{K(p_{hip})\}$', ...
    'interpreter', 'latex', ...
    'FontSize', 12);
set(gca, ...
    'Position', [.12 .21 .84 .75], ...
    'FontSize', 12);
hA = get(gh,'Annotation');
hLL = get([hA{:}],'LegendInformation');
set([hLL{1}],{'IconDisplayStyle'},...
   {'on'}')
set(gh(1),{'DisplayName'},{'Poles'}')
for k=2:P+Ze-1
   set([hLL{k}],{'IconDisplayStyle'},...
   {'off'}')
end
set([hLL{P+Ze}],{'IconDisplayStyle'},...
   {'on'}')
set(gh(P+Ze),{'DisplayName'},{'Zeros'}')
legend('Location','BestOutside')
legend show
hgexport(gcf, [figpath 'poleszeros_lsys_cl.eps']);

figure(398);
% set(gcf, ...
%     'NumberTitle', 'off', ...
%     'Name', 'MEEN 652: Closed Loop Singular Values', ...
%     'Position', [560 528 560 220], ...
%     'PaperPosition', [2 3 4 1.5]);
hold on;
mesh(X, Y, Z(:, :, 1), 0*Z(:,:,1));
mesh(X, Y, Z(:, :, 2), 0*Z(:,:,2) + 1);
hold off;
grid on
view(37.5,30)
set(gca, 'XScale', 'log')
xlabel('Frequency (rad/s)', ...
    'interpreter', 'latex', ...
    'FontSize', 12);
ylabel('$p_{hip}^{x}$ (meters)', ...
    'interpreter', 'latex', ...
    'FontSize', 12);
zlabel('Singular Values (dB)', ...
    'interpreter', 'latex', ...
    'FontSize', 12);
hgexport(gcf, [figpath 'singularvalues_pchip_cl.eps']);

figure(399);
set(gcf, ...
    'NumberTitle', 'off', ...
    'Name', 'MEEN 652: Closed Loop Singular Values', ...
    'Position', [560 528 560 220], ...
    'PaperPosition', [2 3 4 1.5]);
gh = plot(W, Z(1,:,1), W, Z(1,:,2), ...
    'LineWidth', 1);
set(gca, 'XScale', 'log')
xlabel('Frequency (rad/s)', ...
    'interpreter', 'latex', ...
    'FontSize', 12);
ylabel('Singular Values (dB)', ...
    'interpreter', 'latex', ...
    'FontSize', 12);
set(gca, ...
    'Position', [.12 .21 .84 .75], ...
    'FontSize', 12);
grid on
hgexport(gcf, [figpath 'singularvalues_cl.eps']);

figure(400);
set(gcf, ...
    'NumberTitle', 'off', ...
    'Name', 'MEEN 652: Closed Loop Sensitivity TFM Singular Values', ...
    'Position', [560 528 560 220], ...
    'PaperPosition', [2 3 4 1.5]);
gh = plot(W, mag2db(min(sensitiv)), ...
    'LineWidth', 1);
set(gca, 'XScale', 'log')
xlabel('Frequency (rad/s)', ...
    'interpreter', 'latex', ...
    'FontSize', 12);
ylabel('Singular Values (dB)', ...
    'interpreter', 'latex', ...
    'FontSize', 12);
set(gca, ...
    'Position', [.12 .21 .84 .75], ...
    'FontSize', 12);
grid on
hgexport(gcf, [figpath 'singularvalues_STFM_cl.eps']);
